#ifndef TRAJECTORY_H
#define TRAJECTORY_H

#include <ode/ode.h>
#include <QVector>
#include <QFile>
#include <QDebug>
#include "Csv.h"
#include "Macro.h"

class PathPoint
{
public:
	PathPoint(){};
	PathPoint(dReal timeStep, const dReal* pos, const dReal* rot, const dReal* mousePos);
	dReal timeStep;
	dVector3 pos;
	dQuaternion rot;
	dVector3 mousePos;
};

class TotalInfo
{
public:
	dReal timeStep;
	dReal weight;
	dReal mu;
	Vector3d force;
	Vector3d torque;
	Vector3d pos;
	Quaternion<double> rot;
	int status;
	Vector3d conForce;
	TotalInfo(dReal timeStep, dReal weight, dReal mu, Vector3d& force, Vector3d& torque, Vector3d& pos, Quaternion<double>& rot, int status, Vector3d conForce)
	{
		this->timeStep = timeStep;
		this->weight = weight;
		this->mu = mu;
		this->force = force; 
		this->torque = torque; 
		this->pos = pos;
		this->rot = rot;
		this->status = status;
		this->conForce = conForce;
	};
	TotalInfo(){};
	~TotalInfo(){};
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

class Trajectory
{
public:
	QVector<PathPoint> pathPoints;
	std::vector<TotalInfo, Eigen::aligned_allocator<TotalInfo>> totalInfos;
	QString name;
	void record();
	void saveE(QString filename);
	void saveT(QString filename);
	void load(QString filename);
	void pause();
	void reset();
	void addMotorNoise();
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

#endif